#include "smart_car.h"

SmartCar sc = SmartCar(5,9,6,10,A1,A0,A2,A3,A4,A5,2,false);
float clearDistance = 30;
int SR,SL,SR_2,SL_2;
void setup(){
	sc.init();
//  sc.initspeed(150);
}
void button(){
  int val=sc.supersonic.getButtonValue();
  while(!sc.supersonic.getButtonValue())//当按键没被按下时，一直循环
  {
    val=sc.supersonic.getButtonValue();//此句可省略，可让循环跑空
  }
  while(sc.supersonic.getButtonValue())//当按键被按下时
  {
    delay(10);
    val=sc.supersonic.getButtonValue();//读取数字7 口电平值赋给val
    if(val==HIGH)  //第二次判断按键是否被按下
    {
      sc.buzzer.startBeeping();
      while(!sc.supersonic.getButtonValue())  //判断按键是否被松开
        sc.buzzer.stopBeeping();
    }
    else
      sc.buzzer.stopBeeping();
  }
}

void track(int time_run){
    SR = sc.supersonic.getRightLightTrack();//有信号表明在白色区域，车子底板上L3亮；没信号表明压在黑线上，车子底板上L3灭
    SL = sc.supersonic.getLeftLightTrack();//有信号表明在白色区域，车子底板上L2亮；没信号表明压在黑线上，车子底板上L2灭
    if (SL == LOW&&SR==LOW)
      sc.run(time_run);
    else if (SL == HIGH & SR == LOW)
      sc.turnLeftInPlace(0);
    else if (SR == HIGH & SL == LOW)
      sc.turnRightInPlace(0);
    else
      sc.stop(0);
}

void startTrack(){
  button();
  sc.initspeed(185,200,200,200);
  int time_run=0.2;
  while(1)
  {
    track(time_run);
  }  
}
void avoid(){
  button();
  while(1)
  {
    //有信号为LOW  没有信号为HIGH
    
    SR_2 = sc.supersonic.getRightLightAvoid();
    SL_2 = sc.supersonic.getLeftLightAvoid();
    if (SL_2 == HIGH&&SR_2==HIGH)
      sc.run(0);
    else if (SL_2 == HIGH & SR_2 == LOW)
      sc.turnLeftInPlace(0);
      
    else if (SR_2 == HIGH & SL_2 == LOW)
      sc.turnRightInPlace(0);
  //        continue;
    else
      sc.backRightInPlace(0.7);
  }
}

void follow(){
  button();
  while(1)
  {
    //有信号为LOW  没有信号为HIGH
    SR_2 = sc.supersonic.getRightLightAvoid();
    SL_2 = sc.supersonic.getLeftLightAvoid();
    if (SL_2 == LOW&&SR_2==LOW)
      sc.run(0);
    else if (SL_2 == HIGH & SR_2 == LOW)
      sc.turnRightReverse(0);
    else if (SR_2 == HIGH & SL_2 == LOW)
      sc.turnLeftReverse(0);
    else
      sc.stop(0);
  }  
}

void followUltraSonic(){
    int Distance;
    sc.initspeed(110,120,120,120);
    while(1)
    {
      Distance=sc.ultrasonic.frontDetection();
      if(Distance>32)
        sc.run(0);      
      else if(Distance<28&&Distance!=0)
        sc.back(0); 
      else
        sc.stop(0);
    }  
}

int avoidUltraSonic(int Distance,float time_run){
    float left_turn_time=1.2;
    float right_turn_time=0.8;
    float stop_time=1;
    int Left_Distance,Right_Distance;
    if(Distance < 13){
        sc.back(0.3);
        sc.stop(0.7);
        Left_Distance=sc.ultrasonic.leftDetection();
        Right_Distance=sc.ultrasonic.rightDetection();
        if((Left_Distance < 25 ) &&( Right_Distance < 25 )&&(Left_Distance != 0)&&(Right_Distance != 0)){
            sc.backLeftInPlace(left_turn_time);
            sc.stop(0.7);
            sc.backLeftInPlace(left_turn_time);
            return;
        }
        if(Left_Distance >= Right_Distance)//左边比右边空旷
        {      
            sc.turnLeftInPlace(left_turn_time);//左转
            sc.stop(0.7);//刹车，稳定方向
            Right_Distance=Distance;
        }
        else//右边比左边空旷
        {
            sc.turnRightInPlace(right_turn_time);//右转
            sc.stop(0.7);//刹车，稳定方向
            Left_Distance=Distance;
        }

        if(Right_Distance<25&&Right_Distance!=0){
          sc.run(time_run);
          while(Right_Distance<25&&Right_Distance!=0){
            sc.run(time_run);
            Right_Distance=sc.ultrasonic.rightDetection();
          }
//          sc.ultrasonic.frontDetection();
          sc.turnRightInPlace(right_turn_time);
        }
        else if(Left_Distance<25&&Left_Distance!=0){
          sc.run(time_run);
          while(Left_Distance<25&&Left_Distance!=0){
            sc.run(time_run); 
            Left_Distance=sc.ultrasonic.leftDetection();
          }
//          sc.ultrasonic.frontDetection();
          sc.turnLeftInPlace(left_turn_time);
        }
        else{
          sc.run(time_run);
          return ;
        }
    }
    else
      return;
}

void trackWithUltraSonic(){
  button();
  sc.initspeed(85,90,90,90);
  int time_run=0.3;
  int Distance;
  int num=0;
  while(1)
  {
    if(num==5){
      num=0;
      Distance=sc.ultrasonic.frontDetection();
      avoidUltraSonic(Distance,time_run);        
    }
    track(0);
    num++;
  }
}

//void trackWithUltraSonic()
//{
//  button();     //调用按键扫描函数
//  int time_run=0.2;
//  float Front_Distance,Left_Distance,Right_Distance;
//  while(1)
//  { 
//    Front_Distance=sc.ultrasonic.frontDetection();//测量前方距离
//    if(Front_Distance < 20)//当遇到障碍物时
//    {
//      sc.back(1);//后退减速
//      sc.stop(1);//停下来做测距
//      Left_Distance=sc.ultrasonic.leftDetection();//测量左边距障碍物距离
//      Right_Distance=sc.ultrasonic.rightDetection();//测量右边距障碍物距离
//      if((Left_Distance < 20 ) &&( Right_Distance < 20 ))//当左右两侧均有障碍物靠得比较近
//        sc.backLeftInPlace(0.7);//旋转掉头
//      else if(Left_Distance > Right_Distance)//左边比右边空旷
//      {      
//        sc.turnLeftInPlace(1);//左转
//        sc.stop(1);//刹车，稳定方向
//      }
//      else//右边比左边空旷
//      {
//        sc.turnRightInPlace(1);//右转
//        sc.stop(1);//刹车，稳定方向
//      }
//    }
//    else
//      track(time_run);    
//  }
//} 
void loop(){
    setup();

//    trackWithUltraSonic();
    followUltraSonic();
//    startTrack();
//    sc.initspeed(110,120,120,120);
//    sc.run(2);
//    sc.stop(1);
//    sc.back(3);
}
